/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

/**Fleet Formation Constructor
 *
 * Controller which drives a group of machines to a wanted formation. One of the fleet machine is the "master" and the other machines
 * are driven relative to the master.
 *
 * \file
 * \brief
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#ifndef _FLEET_FORMATION_CONSTRUCTOR
#define _FLEET_FORMATION_CONSTRUCTOR


#include "PositionClient.hpp"
#include "SpeedCtrlServer.hpp"
#include "SpeedCtrlClient.hpp"
#include <vector>
#include <map>
#include "owndebug.h"
#include "ownutils.h"
#include "binbag.h"


class CFleetFormationConstructor: private gim::CSync,
                                  private gim::CThread,
                                  public MaCI::Position::CPositionClientCallback, {
public:
  CFleetFormationConstructor();


  ~CFleetFormationConstructor();


  AddMasterPosition(MaCI::Position::PositionClient &aPositionClient);

  AddSlaveMachinePosition(MaCI::Position::TPose2D &aPose,
                          MaCI::Position::PositionClient &aPositionClient,
                          MaCI::SpeedCtlr::SpeedCtrlClient &aSpeedCtrlClient);


  GoToFormation();

  StopGoingToFormation();

  ThreadFunction();

  IsInFormation();


  
  
private:
  MaCI::Position::CPositionClient *iMasterPositionClient;
  std::map<MaCI::Position::CPositionClient *,MaCI::SpeedCtrl::CSpeedCtrlClient *>iSlaveSpeedCtrlMap;
  std::map<MaCI::Position::CPositionClient*,MaCI::Position::TPose2D> iSlaveRelativePositionsMap;

  std::map<MaCI::Position::CPositionClient *, bool> iSlaveIsInFormationMap;
  bool iIsInFormation;


};



#endif
